
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>

#include "ibm2d.h"
#include "util.h"
#include "nsmg.h"
#include "boundary.h"
#include "dlist.h"
#include "output.h"

#define REAL double


void boundary_force(REAL **bdpo, REAL **bd_force, int nt, REAL *ds)
{
	int k;

	for (k = 1; k <= nt; k++)
	{
		bd_force[k][1] = (bdpo[k + 1][1] - bdpo[k][1]) / ds[k + 1] - (bdpo[k][1] - bdpo[k - 1][1]) / ds[k];
		bd_force[k][2] = (bdpo[k + 1][2] - bdpo[k][2]) / ds[k + 1] - (bdpo[k][2] - bdpo[k - 1][2]) / ds[k];
	}
}

void fluid_force(REAL **fxx, REAL **fyy, REAL **bdpo, REAL **bd_force, int nt)
{
	extern int nx, ny;
	extern REAL h;

	int i, j, k, ia, ja;
	REAL h2, d, d1, **fx, **fy;

	h2 = pow(h, 2);

	fx = dmatrix(1, nx, 1, ny);
	fy = dmatrix(1, nx, 1, ny);

	zero_matrix(fx, 1, nx, 1, ny);
	zero_matrix(fy, 1, nx, 1, ny);

	for (k = 1; k <= nt; k++) {

		ia = (int)(bdpo[k][1] / h + 0.5) - 1;
		ja = (int)(bdpo[k][2] / h + 0.5) - 1;

		for (i = ia; i <= ia + 3; i++) {

			d1 = delta((REAL)i - 0.5 - bdpo[k][1] / h) / h2;

			for (j = ja; j <= ja + 3; j++) {

				d = d1 * delta((REAL)j - 0.5 - bdpo[k][2] / h);

				fx[i][j] += d * bd_force[k][1];
				fy[i][j] += d * bd_force[k][2];
			}
		}
	}

	ijloop{

		if (i == nx)
			fxx[0][j] = fxx[nx][j] = 0.5*(fx[nx][j] + fx[1][j]);
		else
			fxx[i][j] = 0.5*(fx[i][j] + fx[i + 1][j]);

		if (j == ny)
			fyy[i][0] = fyy[i][ny] = 0.5*(fy[i][ny] + fy[i][1]);
		else
			fyy[i][j] = 0.5*(fy[i][j] + fy[i][j + 1]);

	}

	free_dmatrix(fx, 1, nx, 1, ny);
	free_dmatrix(fy, 1, nx, 1, ny);
}



void move_boundary(REAL **bdpo, int nt, REAL **u, REAL **v)
{
	extern REAL h, dt;

	int i, j, k, ia, ja;
	REAL d, d1, **bd_vel;

	bd_vel = dmatrix(1, nt, 1, 2);

	for (k = 1; k <= nt; k++) {

		bd_vel[k][1] = 0.0;
		bd_vel[k][2] = 0.0;

		ia = (int)(bdpo[k][1] / h + 0.5) - 1;
		ja = (int)(bdpo[k][2] / h + 0.5) - 1;

		for (i = ia; i <= ia + 3; i++) {
			d = delta((REAL)i - 0.5 - bdpo[k][1] / h);

			for (j = ja; j <= ja + 3; j++) {
				d1 = d * delta((REAL)j - 0.5 - bdpo[k][2] / h);

				bd_vel[k][1] = bd_vel[k][1] + 0.5*(u[i][j] + u[i - 1][j])*d1;
				bd_vel[k][2] = bd_vel[k][2] + 0.5*(v[i][j] + v[i][j - 1])*d1;
			}
		}
	}

	for (k = 1; k <= nt; k++) {
		bdpo[k][1] = bdpo[k][1] + dt * bd_vel[k][1];
		bdpo[k][2] = bdpo[k][2] + dt * bd_vel[k][2];
	}

	augment_bdpo(bdpo, nt);

	free_dmatrix(bd_vel, 1, nt, 1, 2);
}


void calc_bdry_normal(REAL **bd_posi, REAL **bd_unit_normal, REAL *ds, int nt)
{
	int i;
	REAL dx, dy, coeff[3], p[3][2];

	make_ds(ds, bd_posi, nt);  // 20210930

	for (i = 1; i <= nt; i++) {

		p[0][0] = bd_posi[i - 1][1];
		p[1][0] = bd_posi[i][1];
		p[2][0] = bd_posi[i + 1][1];

		p[0][1] = bd_posi[i - 1][2];
		p[1][1] = bd_posi[i][2];
		p[2][1] = bd_posi[i + 1][2];

		quadratic_approximation(coeff, p[0][0], p[1][0], p[2][0], ds[i], ds[i + 1]);
		dx = coeff[1];

		quadratic_approximation(coeff, p[0][1], p[1][1], p[2][1], ds[i], ds[i + 1]);
		dy = coeff[1];

		bd_unit_normal[i][1] = dy / sqrt(pow(dx, 2) + pow(dy, 2));
		bd_unit_normal[i][2] = -dx / sqrt(pow(dx, 2) + pow(dy, 2));

		//printf("bd_norm[%d] = %f\n", i, sqrt(pow(dx, 2) + pow(dy, 2)));        
	}

	bd_unit_normal[0][1] = bd_unit_normal[nt][1];
	bd_unit_normal[0][2] = bd_unit_normal[nt][2];

	bd_unit_normal[nt + 1][1] = bd_unit_normal[1][1];
	bd_unit_normal[nt + 1][2] = bd_unit_normal[1][2];

}


void calc_bdry_normal_curvature(REAL **bd_posi, REAL **bd_unit_normal, REAL *bd_curvature, REAL *ds, int nt)
{
	int i;
	REAL coeff[3], p[3][2];

	REAL dx, dxx, dy, dyy;

	//augment_bdpo(bd_posi, nt); no deed to augment     
	//make_ds(ds, bd_posi, nt);

	for (i = 1; i <= nt; i++) {

		p[0][0] = bd_posi[i - 1][1];
		p[1][0] = bd_posi[i][1];
		p[2][0] = bd_posi[i + 1][1];

		p[0][1] = bd_posi[i - 1][2];
		p[1][1] = bd_posi[i][2];
		p[2][1] = bd_posi[i + 1][2];

		quadratic_approximation(coeff, p[0][0], p[1][0], p[2][0], ds[i], ds[i + 1]);
		dx = coeff[1];
		dxx = 2.0*coeff[2];

		quadratic_approximation(coeff, p[0][1], p[1][1], p[2][1], ds[i], ds[i + 1]);
		dy = coeff[1];
		dyy = 2.0*coeff[2];

		bd_unit_normal[i][1] = ( dy / sqrt(pow(dx, 2) + pow(dy, 2)));
		bd_unit_normal[i][2] = (-dx / sqrt(pow(dx, 2) + pow(dy, 2)));

		bd_curvature[i] = fabs((dxx*dy - dx * dyy) / sqrt(pow(pow(dx, 2) + pow(dy, 2), 3)));

		//printf("bd_norm[%d] = %f\n", i, sqrt(pow(dx, 2) + pow(dy, 2)));
		//printf("bd_curvature[%d] = %f\n", i, bd_curvature[i]);
	}

	bd_unit_normal[0][1] = bd_unit_normal[nt][1];
	bd_unit_normal[0][2] = bd_unit_normal[nt][2];

	bd_unit_normal[nt + 1][1] = bd_unit_normal[1][1];
	bd_unit_normal[nt + 1][2] = bd_unit_normal[1][2];

	bd_curvature[0] = bd_curvature[nt];
	bd_curvature[nt + 1] = bd_curvature[1];
}



void calc_bdry_normal_curvature2(REAL **bd_posi, REAL **bd_unit_normal, REAL *bd_curvature, REAL *ds, int nt)  // without qudratic polynomial approximation 
{
	int i;
	REAL dx, dy, dss1, dss2;

	//make_ds(ds, bd_posi, nt);
	//augment_bdpo(bd_posi, nt);

	for (i = 1; i <= nt; i++) {

		dx = bd_posi[i + 1][1] - bd_posi[i][1];
		dy = bd_posi[i + 1][2] - bd_posi[i][2];

		bd_unit_normal[i][1] = dy / sqrt(pow(dx, 2) + pow(dy, 2));
		bd_unit_normal[i][2] = -dx / sqrt(pow(dx, 2) + pow(dy, 2));

		dss1 = ((bd_posi[i + 1][1] - bd_posi[i][1]) / ds[i + 1] - (bd_posi[i][1] - bd_posi[i - 1][1]) / ds[i]) / (ds[i] + ds[i + 1]) / 2.0;
		dss2 = ((bd_posi[i + 1][2] - bd_posi[i][2]) / ds[i + 1] - (bd_posi[i][2] - bd_posi[i - 1][2]) / ds[i]) / (ds[i] + ds[i + 1]) / 2.0;

		REAL dssnorm = sqrt(pow(dss1, 2) + pow(dss2, 2));

		bd_unit_normal[i][1] = dss1 / dssnorm;
		bd_unit_normal[i][2] = dss2 / dssnorm;

		//bd_curvature[i] = fabs((dss1*dy - dx * dss2) / sqrt(pow(pow(dx, 2) + pow(dy, 2), 3)));
		bd_curvature[i] = dssnorm;

		//printf("bd_curvature[%d] = %f\n", i, bd_curvature[i]);
	}

	bd_unit_normal[0][1] = bd_unit_normal[nt][1];
	bd_unit_normal[0][2] = bd_unit_normal[nt][2];

	bd_unit_normal[nt + 1][1] = bd_unit_normal[1][1];
	bd_unit_normal[nt + 1][2] = bd_unit_normal[1][2];

	bd_curvature[0] = bd_curvature[nt];
	bd_curvature[nt + 1] = bd_curvature[1];
}


REAL calc_area(REAL **bdpo, int nt)
{
	int k;
	REAL value;

	value = 0.0;
	for (k = 1; k <= nt; k++) {
		value += bdpo[k][1] * bdpo[k + 1][2] - bdpo[k][2] * bdpo[k + 1][1];
	}

	return 0.5*fabs(value);
}




void repair_bd(REAL **bdpo, REAL **bd_unit_normal, REAL ini_area, REAL area, int nt)
{
	extern REAL epsilon;

	int k;
	REAL alpha, beta;

	alpha = 0.0;
	beta = 0.0;

	for (k = 1; k <= nt; k++)
	{
		alpha = alpha + bd_unit_normal[k][1] * bd_unit_normal[k + 1][2] - bd_unit_normal[k][2] * bd_unit_normal[k + 1][1];
		beta = beta + bd_unit_normal[k][1] * bdpo[k + 1][2] + bd_unit_normal[k + 1][2] * bdpo[k][1]
			- bd_unit_normal[k][2] * bdpo[k + 1][1] - bd_unit_normal[k + 1][1] * bdpo[k][2];
	}

	alpha = 0.5*alpha;
	beta = 0.5*beta;

	epsilon = (-beta + sqrt(pow(beta, 2) - 4.0 * alpha * (area - ini_area))) / (2.0 * alpha);

	for (k = 1; k <= nt; k++) {
		bdpo[k][1] = bdpo[k][1] + epsilon * bd_unit_normal[k][1];
		bdpo[k][2] = bdpo[k][2] + epsilon * bd_unit_normal[k][2];
	}

	augment_bdpo(bdpo, nt);

}




void repair_bd_curvature(REAL **bdpo, REAL **bd_unit_normal, REAL *bd_curvature, REAL ini_area, REAL area, int nt)
{
	extern REAL epsilon;

	int k;
	REAL alpha, beta;
	REAL kn01, kn02, kn11, kn12;

	alpha = 0.0;
	beta = 0.0;

	for (k = 1; k <= nt; k++)
	{
		kn01 = bd_curvature[k] * bd_unit_normal[k][1];
		kn02 = bd_curvature[k] * bd_unit_normal[k][2];
		kn11 = bd_curvature[k + 1] * bd_unit_normal[k + 1][1];
		kn12 = bd_curvature[k + 1] * bd_unit_normal[k + 1][2];

		alpha = alpha + kn01 * kn12 - kn02 * kn11;
		beta = beta + kn01 * bdpo[k + 1][2] + kn12 * bdpo[k][1] - kn02 * bdpo[k + 1][1] - kn11 * bdpo[k][2];
	}

	alpha = 0.5 * alpha;
	beta = 0.5 * beta;

	epsilon = (-beta + sqrt(pow(beta, 2) - 4.0 * alpha * (area - ini_area))) / (2.0 * alpha);

	for (k = 1; k <= nt; k++)
	{
		kn01 = bd_curvature[k] * bd_unit_normal[k][1];
		kn02 = bd_curvature[k] * bd_unit_normal[k][2];

		bdpo[k][1] = bdpo[k][1] + epsilon * kn01;
		bdpo[k][2] = bdpo[k][2] + epsilon * kn02;
	}

	augment_bdpo(bdpo, nt);
}




void make_ds(REAL *ds, REAL **p, int npo)
{
	int i;
	for (i = 1; i <= npo + 1; i++) {
		ds[i] = sqrt(pow(p[i][1] - p[i - 1][1], 2) + pow(p[i][2] - p[i - 1][2], 2));
	}
	ds[0] = ds[npo + 1];
}


void check_boundary(REAL ***ref_bdpo, REAL **ref_ds, REAL ds_max, REAL ds_min, int *ref_nt)
{
	BN *list;
	int length;

	int input_nt = *ref_nt;
	REAL** bdpo = *ref_bdpo;
	REAL *ds = *ref_ds;

	make_ds(ds, bdpo, input_nt);

	init_list(bdpo, &list, input_nt);

	printf(" list_npo_befor_check = %d\n", input_nt);

	free_dmatrix(bdpo, -1, input_nt, 1, 2);
	free_dvector(ds, 0, input_nt + 1);

	length = check_list(&list, ds_max, ds_min);

	//display(list);//for test	

	bdpo = dmatrix(-1, length + 1, 1, 2);
	ds = dvector(0, length + 1);

	list2matrix(bdpo, list, 1); // 1 is offset

	printf(" list_npo_after_check = %d\n", length);

	augment_bdpo(bdpo, length);
	make_ds(ds, bdpo, length);

	*ref_nt = length;
	*ref_bdpo = bdpo;
	*ref_ds = ds;

	destroy_list(list);
}



void make_center(REAL **bdpo, int n_po, REAL center[])
{
	int i, j;
	for (i = 1; i <= 2; i++)
		center[i] = 0.0;

	for (i = 1; i <= n_po; i++) {
		for (j = 1; j <= 2; j++) {
			center[j] += bdpo[i][j];
		}
	}

	for (i = 1; i <= 2; i++)
		center[i] /= (REAL)n_po;
}

REAL calc_bdry_length(REAL **p, int nt)
{
	int k;
	REAL value;

	value = 0.0;
	for (k = 1; k <= nt; k++) {
		value += sqrt(pow(p[k + 1][1] - p[k][1], 2) + pow(p[k + 1][2] - p[k][2], 2));
	}

	return value;
}


void area_correct(REAL **bdpo, REAL **bd_unit_normal, REAL *ds, int nt, REAL ini_area, REAL area)
{

	calc_bdry_normal(bdpo, bd_unit_normal, ds, nt);

	repair_bd(bdpo, bd_unit_normal, ini_area, area, nt);

	printf(" --- area correct by normal vector!\n");
}


void area_correct_normal_curvature(REAL **bdpo, REAL **bd_unit_normal, REAL *bd_curvature, REAL *ds, int nt, REAL ini_area, REAL area)
{

	calc_bdry_normal_curvature(bdpo, bd_unit_normal, bd_curvature, ds, nt);

	repair_bd_curvature(bdpo, bd_unit_normal, bd_curvature, ini_area, area, nt);

	printf(" --- area correct by curvature vector!\n");
}


void augment_bdpo(REAL **bdpo, int nt)
{
	bdpo[-1][1] = bdpo[nt - 1][1];
	bdpo[-1][2] = bdpo[nt - 1][2];
	bdpo[0][1] = bdpo[nt][1];
	bdpo[0][2] = bdpo[nt][2];
	bdpo[nt + 1][1] = bdpo[1][1];
	bdpo[nt + 1][2] = bdpo[1][2];
}




////////////////////////// Delta functions test. choose 4-point ones to use  ///////////////////////////////////

//REAL delta(REAL x)   //  4-point hat function      // curvature oscillation        
//{
//	REAL value;
//	if (fabs(x) > 2.0)
//		value = 0.0;
//	else
//		value = 0.5 - 0.25 * fabs(x);
//	return value;
//}

REAL delta(REAL x)    // 4-point delta function // for old two-phase flows we are using !!!!
{
	REAL value;

	if (fabs(x) > 2.0)
		value = 0.0;
	else if (fabs(x) < 1.0)
		value = (3.0 - 2.0*fabs(x) + sqrt(1.0 + 4.0*fabs(x) - 4.0*x*x))*0.125;
	else
		value = 0.5 - (3.0 - 2.0*(2.0 - fabs(x)) + sqrt(1.0 + 4.0*(2.0 - fabs(x)) - 4.0*pow(2.0 - fabs(x), 2)))*0.125;
	return value;
}

//REAL delta(REAL x)   // 3-point delta function        // similar to the one we are using, some sections oscillation for curvature curve         
//{
//	REAL value;
//	if (fabs(x) > 1.5)
//		value = 0.0;
//	else if (fabs(x) <= 0.5)
//		value = (1.0 + sqrt(-3.0 * pow(x, 2) + 1.0)) / 3.0;
//	else
//		value = (5.0 - 3.0 * fabs(x) - sqrt(-3.0 * pow(1.0 - fabs(x), 2) + 1.0)) / 6.0;
//	return value;
//}

//REAL delta(REAL x)   // smoothed 4-point delta function  // serious curvature oscillation 
//{
//	REAL value;
//	if (fabs(x) >= 2.5)
//		value = 0.0;
//	else if (fabs(x) < 2.5 && fabs(x) >= 1.5)
//		value = 17.0 / 16 - pi / 64.0 - 0.75*fabs(x) + 0.125*x*x + (fabs(x) - 2.0)*sqrt(-14.0 + 16.0*fabs(x) - 4.0*x*x) / 16.0 + asin(sqrt(2)*(fabs(x) - 2.0)) / 16.0;
//	else if (fabs(x) < 1.5 && fabs(x) >= 0.5)
//		value = 0.25 + 0.125*(1 - fabs(x))*sqrt(-2.0 + 8.0*fabs(x) - 4.0*x*x) - 0.125*asin(sqrt(2)*(fabs(x) - 1.0));
//	else
//		value = 3.0 / 8 + pi / 32.0 - 0.25*x*x;
//	return value;
//}

//REAL delta(REAL x)   // 4-point cosine function     // serious curvature oscillation     
//{
//	REAL value;
//	if (fabs(x) > 2.0)
//		value = 0.0;
//	else
//		value = 0.25 * (1.0 + cos(0.5 * pi * x));
//	return value;
//}

//REAL delta(REAL x)   //  smoothed  4-point cosine function     // serious curvature oscillation 
//{
//	REAL value;
//	if (fabs(x) > 2.5)
//		value = 0.0;
//	else if (fabs(x) < 1.5)
//		value = 0.25 * (pi + 2.0 * sin(0.25 * pi * (2.0 * x + 1.0)) - 2.0 * sin(0.25 * pi * (2.0 * x - 1.0))) / pi;
//	else
//		value = -0.125 * (-5.0*pi + 2.0*pi*fabs(x) + 4.0*sin(0.25*pi*(2.0*fabs(x)-1.0 ))) / pi;
//	return value;
//}



//REAL delta(REAL x)   //  smoothed  3-point cosine function   // no large oscillation, similar to the 4-point function that is using now 
//{
//	REAL value;
//	if (fabs(x) > 2.0)
//		value = 0.0;
//	else if (fabs(x) < 1.0)
//		value = 17.0 / 48 + sqrt(3.0) * pi / 108 + 0.25 * fabs(x) - 0.25 * x * x + (1.0 - 2.0 * fabs(x)) / 16.0 * sqrt(-12.0 * x * x + 12.0 * fabs(x) + 1.0) 
//		      - sqrt(3.0) * asin(0.5 * sqrt(3.0) * (2.0 * fabs(x) - 1.0)) / 12.0;
//	else 
//		value = 55.0 / 48 - sqrt(3.0) * pi / 108 - 13.0 * fabs(x)/12 + 0.25 * x * x + (2.0 * fabs(x)-3.0) / 48.0 * sqrt(-12.0 * x * x + 36.0 * fabs(x) - 23.0)
//		      + sqrt(3.0) * asin(0.5 * sqrt(3.0) * (2.0 * fabs(x) - 3.0)) / 36.0;
//	return value;
//}

//REAL delta(REAL x)      // 2-point hat functin    // serious curvature oscillation     
//{
//	REAL value;
//	if (fabs(x) > 1)
//		value = 0.0;
//	else
//		value = 1 - fabs(x);
//	return value;
//}

//REAL delta(REAL x)   //  smoothed  2-point  function          oscillation not serious
//{
//	REAL value;
//	if (fabs(x) > 1.5)
//		value = 0.0;
//	else if (fabs(x) < 0.5)
//		value = 0.75 - x * x;
//	else
//		value = 9.0 / 8 - 1.5 * fabs(x) + 0.5 * x * x;
//	return value;
//}

//////////////////////////////////// end Delta function  ////////////////////////////////////// 




//REAL quadratic(REAL x, REAL c[])
//{
//	return c[2] * pow(x, 2) + c[1] * x + c[0];
//}


//void quadratic_approximation(REAL c[], REAL p0, REAL p1, REAL p2, REAL ds1, REAL ds2)    // old one, use i <-> Delta s
//{
//	REAL ds12, rds1, rds2, rds12;
//	ds12 = ds1 + ds2;
//
//	rds1 = 1.0 / ds1;
//	rds2 = 1.0 / ds2;
//	rds12 = 1.0 / ds12;
//
//	c[2] = p0 * rds1*rds12 - p1 * rds1*rds2 + p2 * rds2*rds12;
//	c[1] = -p0 * (2.0*ds1 + ds2)*rds1*rds12 + p1 * (ds1 + ds2)*rds1*rds2 - p2 * ds1*rds2*rds12;
//	c[0] = p0;
//
//}
//void calc_bdry_normal_curvature(REAL **bd_posi, REAL **bd_unit_normal, REAL *bd_curvature, REAL *ds, int nt)   // old one, use i <-> Delta s 
//{
//	int i;
//	REAL coeff[3], p[3][2];
//
//	REAL dx, dxx, dy, dyy;
//
//	make_ds(ds, bd_posi, nt);
//
//	for (i = 1; i <= nt; i++) {
//
//		p[0][0] = bd_posi[i - 1][1];
//		p[1][0] = bd_posi[i][1];
//		p[2][0] = bd_posi[i + 1][1];
//
//		p[0][1] = bd_posi[i - 1][2];
//		p[1][1] = bd_posi[i][2];
//		p[2][1] = bd_posi[i + 1][2];
//
//		quadratic_approximation(coeff, p[0][0], p[1][0], p[2][0], ds[i], ds[i + 1]);
//		dx  = 2.0*coeff[2] * ds[i] + coeff[1];		
//		dxx = 2.0*coeff[2];
//
//		quadratic_approximation(coeff, p[0][1], p[1][1], p[2][1], ds[i], ds[i + 1]);
//		dy  = 2.0*coeff[2] * ds[i] + coeff[1];
//		dyy = 2.0*coeff[2];
//
//		bd_unit_normal[i][1] = (dy / sqrt(pow(dx, 2) + pow(dy, 2)));
//		bd_unit_normal[i][2] = (-dx / sqrt(pow(dx, 2) + pow(dy, 2)));
//
//		bd_curvature[i] = fabs((dxx*dy - dx * dyy) / sqrt(pow(pow(dx, 2) + pow(dy, 2), 3)));
//
//		//printf("bd_norm[%d] = %f\n", i, sqrt(pow(dx, 2) + pow(dy, 2)));
//		//printf("bd_curvature[%d] = %f\n", i, bd_curvature[i]);
//	}
//
//	bd_unit_normal[0][1] = bd_unit_normal[nt][1];
//	bd_unit_normal[0][2] = bd_unit_normal[nt][2];
//
//	bd_unit_normal[nt + 1][1] = bd_unit_normal[1][1];
//	bd_unit_normal[nt + 1][2] = bd_unit_normal[1][2];
//
//	bd_curvature[0] = bd_curvature[nt];
//	bd_curvature[nt + 1] = bd_curvature[1];
//}

//void calc_bdry_normal(REAL **bd_posi, REAL **bd_unit_normal, REAL *ds, int nt)    // old one, use i <-> Delta s   
//{
//	int i;
//	REAL a, b, coeff[3], p[3][2];
//
//	for (i = 1; i <= nt; i++) {
//
//		p[0][0] = bd_posi[i - 1][1];
//		p[1][0] = bd_posi[i][1];
//		p[2][0] = bd_posi[i + 1][1];
//
//		p[0][1] = bd_posi[i - 1][2];
//		p[1][1] = bd_posi[i][2];
//		p[2][1] = bd_posi[i + 1][2];
//
//		quadratic_approximation(coeff, p[0][0], p[1][0], p[2][0], ds[i], ds[i + 1]);
//		b = 2.0*coeff[2] * ds[i] + coeff[1];
//
//		quadratic_approximation(coeff, p[0][1], p[1][1], p[2][1], ds[i], ds[i + 1]);
//		a = -(2.0*coeff[2] * ds[i] + coeff[1]);
//
//
//		bd_unit_normal[i][1] = -a / sqrt(pow(a, 2) + pow(b, 2));
//		bd_unit_normal[i][2] = -b / sqrt(pow(a, 2) + pow(b, 2));
//
//	}
//
//	bd_unit_normal[0][1] = bd_unit_normal[nt][1];
//	bd_unit_normal[0][2] = bd_unit_normal[nt][2];
//
//	bd_unit_normal[nt + 1][1] = bd_unit_normal[1][1];
//	bd_unit_normal[nt + 1][2] = bd_unit_normal[1][2];
//
//}


//void quadratic_approximation(REAL c[], REAL p0, REAL p1, REAL p2, REAL ds1, REAL ds2)
//{
//	REAL ds12, rds1, rds2, rds12;
//	ds12 = ds1 + ds2;
//
//	rds1  = 1.0 / ds1;
//	rds2  = 1.0 / ds2;
//	rds12 = 1.0 / ds12;
//
//	c[2] =  p0*rds1*rds12     - p1*rds1*rds2             + p2*rds2*rds12;
//	c[1] = -p0*ds2*rds1*rds12 - p1*(ds1 - ds2)*rds1*rds2 + p2*ds1*rds2*rds12;
//	c[0] =  p1;
//}

